# Find Circles Example
#
# This example shows off how to find circles in the image using the Hough
# Transform. https://en.wikipedia.org/wiki/Circle_Hough_Transform
#
# Note that the find_circles() method will only find circles which are completely
# inside of the image. Circles which go outside of the image/roi are ignored...

import image, math, pyb, sensor, struct, time
from pid import PID

RED_LED_PIN = 1
BLUE_LED_PIN = 3

#############################################
# class name:KalmanF
# description:
# Kalman Filter simplified version for micrio python
# on openmv
# author:
# Process (SHAOCHANG TAN)
# 478710209@qq.com
#############################################
class KalmanF:
    def __init__(self,int_observation,cov,A):
        self.A = A
        self.cov = cov
        self.P_k = cov
        self.H_k = 0
        self.observation = int_observation
        self.real = int_observation
        self.prediction = int_observation

    def smooth(self,observation):
        self.observation = observation
        self.prediction = self.A * self.real
        self.P_k = math.sqrt( (1-self.H_k)*self.P_k )

        self.H_k = (self.P_k*self.P_k) / ( (self.P_k*self.P_k)+(self.cov*self.cov) )
        self.real = self.H_k * self.observation + (1-self.H_k) * self.prediction

        return self.real

#############################################



pyb.LED(RED_LED_PIN).on()

pid_front = PID(p=5, i=0.1, imax=4)
pid_right = PID(p=5, i=0.1, imax=4)

uart_baudrate = 115200

lens_mm = 2.8 # Standard Lens.
lens_to_camera_mm = 22 # Standard Lens.
sensor_w_mm = 3.984 # For OV7725 sensor - see datasheet.
sensor_h_mm = 2.952 # For OV7725 sensor - see datasheet.

sensor.reset()
sensor.set_pixformat(sensor.RGB565) # grayscale is faster
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(time = 2000)
clock = time.clock()

R_real_cirle = 35 #35mm

x_res = 160 # QQVGA
y_res = 120 # QQVGA
f_x = (lens_mm / sensor_w_mm) * x_res
f_y = (lens_mm / sensor_h_mm) * y_res
c_x = x_res / 2
c_y = y_res / 2
h_fov = 2 * math.atan((sensor_w_mm / 2) / lens_mm)
v_fov = 2 * math.atan((sensor_h_mm / 2) / lens_mm)

# Link Setup

uart = pyb.UART(3, uart_baudrate, timeout_char = 1000)



def circle_is_red(c):
    area = (c.x()-c.r(),c.y()-c.r(),2*c.r(),2*c.r())
    statistics = img.get_statistics(roi=area)
    print('L:',statistics.l_mode(),'A:',statistics.a_mode(),'B:',statistics.b_mode())
    if (50<statistics.l_mode()<100 \
          and -30<statistics.a_mode()<30 \
          and -30<statistics.b_mode()<30):#if the circle is red
        return 1
    else:
        return 0

def checksum(data, extra): # https://github.com/mavlink/c_library_v1/blob/master/checksum.h
    output = 0xFFFF
    for i in range(len(data)):
        tmp = data[i] ^ (output & 0xFF)
        tmp = (tmp ^ (tmp << 4)) & 0xFF
        output = ((output >> 8) ^ (tmp << 8) ^ (tmp << 3) ^ (tmp >> 4)) & 0xFFFF
    tmp = extra ^ (output & 0xFF)
    tmp = (tmp ^ (tmp << 4)) & 0xFF
    output = ((output >> 8) ^ (tmp << 8) ^ (tmp << 3) ^ (tmp >> 4)) & 0xFFFF
    return output


#about MAVlink
packet_sequence = 0
SET_POSITION_TARGET_LOCAL_NED_msg_id = 84
SET_POSITION_TARGET_LOCAL_NED_crc_extra = 143 #PX4固件里找 mavlink_msg_set_position_target_local_ned.h 里面有这个
MAV_FRAME_BODY_FRD = 8
MAV_system_id = 1
MAV_COMP_ID_AUTOPILOT = 1

TYPE_MASK = 4039 #only vx vy vz

def send_mavlink_cmd_vel(vx,vy):
    global packet_sequence
    temp = struct.pack("<lfffffffffffhbbb",
                        0,
                        0,0,0,
                        vx,vy,0,
                        0,0,0,
                        0,0,
                        TYPE_MASK,
                        MAV_system_id,
                        MAV_COMP_ID_AUTOPILOT,
                        MAV_FRAME_BODY_FRD,

                        )
    temp = struct.pack("<bbbbb53s",
                        53,
                        packet_sequence & 0xFF,
                        MAV_system_id,
                        MAV_COMP_ID_AUTOPILOT,
                        SET_POSITION_TARGET_LOCAL_NED_msg_id,
                        temp
                        )
    temp = struct.pack("<b58sh",
                        0xFE,
                        temp,
                        checksum(temp,SET_POSITION_TARGET_LOCAL_NED_crc_extra)
                        )
    packet_sequence += 1
    uart.write(temp)

    # print(temp)


while(True):
    clock.tick()
    img = sensor.snapshot().lens_corr(1.8)
    w = img.width()
    h = img.height()

    pyb.LED(BLUE_LED_PIN).off()
    front_err = 0
    right_err = 0
    v_front = 0
    v_right = 0

    # Circle objects have four values: x, y, r (radius), and magnitude. The
    # magnitude is the strength of the detection of the circle. Higher is
    # better...

    # `threshold` controls how many circles are found. Increase its value
    # to decrease the number of circles detected...

    # `x_margin`, `y_margin`, and `r_margin` control the merging of similar
    # circles in the x, y, and r (radius) directions.

    # r_min, r_max, and r_step control what radiuses of circles are tested.
    # Shrinking the number of tested circle radiuses yields a big performance boost.

    for c in img.find_circles(threshold = 3600, x_margin = 50, y_margin = 50, r_margin = 20,
            r_min = 5, r_max = 100, r_step = 2):

        if circle_is_red(c):
            img.draw_circle(c.x(), c.y(), c.r(), color = (255, 0, 0))

            right_err = ((c.x() / w) - 0.5) * h_fov #0.05设置成阈值  ， 视野右下方为++
            front_err = -((c.y() / h) - 0.5) * v_fov


            if abs(front_err)>0.1:
                v_front = pid_front.get_pid(front_err,1)
                pyb.LED(BLUE_LED_PIN).on()
            if abs(right_err)>0.1:
                v_right = pid_right.get_pid(right_err,1)

            print('front_err:',front_err,'right_err:',right_err,'\r\n'
                    ,'v_front:',v_front,'v_right:',v_right)

            send_mavlink_cmd_vel(v_front,v_right)


            #print( ((c.x() / w) - 0.5) * h_fov  , ((c.y() / h) - 0.5) * v_fov )
        else:
            img.draw_circle(c.x(), c.y(), c.r(), color = (255, 255, 255))


    #print("FPS %f" % clock.fps())
